import rospy
from geometry_msgs.msg import PoseStamped, Point
from nav_msgs.msg import Odometry
import math
import tf
from pyquaternion import Quaternion
import sys


local_pose = PoseStamped()
local_pose.header.frame_id = 'world'

#need change
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
q = Quaternion([quaternion[3],quaternion[0],quaternion[1],quaternion[2]])


def Dongbu_callback(data):    
    local_pose.pose.position.x = data.pose.position.x
    local_pose.pose.position.y = data.pose.position.y
    local_pose.pose.position.z = data.pose.position.z
    
    q_= Quaternion([data.pose.orientation.w,data.pose.orientation.x,data.pose.orientation.y,data.pose.orientation.z])
    q_ = q_*q

    local_pose.pose.orientation.w = q_[0]
    local_pose.pose.orientation.x = q_[1]
    local_pose.pose.orientation.y = q_[2]
    local_pose.pose.orientation.z = q_[3]
    
rospy.init_node('DongBu_node')
rospy.Subscriber("/vrpn_client_node/Rigid0/pose", PoseStamped, Dongbu_callback,queue_size=1)
position_pub = rospy.Publisher("/mavros/vision_pose/pose", PoseStamped, queue_size=1)
rate = rospy.Rate(30) 

while not rospy.is_shutdown():
    if (local_pose.pose.position == Point()):
        continue
    else:
        print("Vins pose received")
        local_pose.header.stamp = rospy.Time.now()
        position_pub.publish(local_pose) 
    try:
        rate.sleep()
    except:
        continue
  

